//==================================================================
//  Copyright (C) 2006  Davide Pasca
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//
//  This library is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//==================================================================
/// @file	pmatrix.cpp
///
/// 
///
//==================================================================

//==================================================================

#define _USE_MATH_DEFINES
#include <math.h>
#include "pmatrix.h"

//==================================================================
using namespace PMath;

//==================================================================
void Matrix44::BackTransform()
{
	float x = _m[3][0];
	float y = _m[3][1];
	float z = _m[3][2];

	float	tmp;
	tmp = _m[0][1];	_m[0][1] = _m[1][0];	_m[1][0] = tmp;
	tmp = _m[0][2];	_m[0][2] = _m[2][0];	_m[2][0] = tmp;
	tmp = _m[1][2];	_m[1][2] = _m[2][1];	_m[2][1] = tmp;

	_m[3][0] = -( _m[0][0] * x + _m[1][0] * y + _m[2][0] * z );
	_m[3][1] = -( _m[0][1] * x + _m[1][1] * y + _m[2][1] * z );
	_m[3][2] = -( _m[0][2] * x + _m[1][2] * y + _m[2][2] * z );
}

//==================================================================
Matrix44 Matrix44::GetBackProjectedPersp() const
{
	float a = _m[0][0];
	float b = _m[1][1];
	float c = _m[2][2];
	float d = _m[3][2];
	float e = _m[2][3];

	return Matrix44(
		1/a,	0,		0,		0,
		0,		1/b,	0,		0,
		0,		0,		0,		1/d,
		0,		0,		1/e,	-c/(d*e) );
}


//==================================================================
void Matrix44::CopyFromOGL( const float ogl_mat[16] )
{
	for (int i=0; i < 16; ++i)
		((float *)_m)[i] = (float)ogl_mat[i];
}

//=============================================================================
Matrix44 Matrix44::Transpose() const
{
	Matrix44	tmp;

	for (int i=0; i < 4; ++i)
		for (int j=0; j < 4; ++j)
			tmp._m[i][j] = _m[j][i];

	return tmp;
}

//=============================================================================
Matrix44 Matrix44::Transpose33() const
{
	Matrix44	tmp;

	tmp.Identity();
	for (int i=0; i < 3; ++i)
		for (int j=0; j < 3; ++j)
			tmp._m[i][j] = _m[j][i];

	return tmp;
}

/*
//=============================================================================
void Matrix44::SetOrthogonalProjInv( float x1, float x2, float y2, float y1, float n, float f )
{
	Identity();

	float dx = x2 - x1;
	float dy = y1 - y2;
	float dz = f - n;

	PASSERT( dx != 0 && dy != 0 && dz != 0 );

	_m11 = dx;
	_m22 = dy;
	_m33 = dz * -0.5f;

	_m31 = (x2 + x1) * 0.5f;
	_m32 = (y1 + y2) * 0.5f;
	_m33 = (n + f) * 0.5f;
}
*/

//==================================================================
static PFORCEINLINE float det2x2( float a, float b, float c, float d )
{
	return a * d - b * c;
}

//==================================================================
/*
* float = det3x3(  a1, a2, a3, b1, b2, b3, c1, c2, c3 )
* 
* calculate the determinant of a 3x3 matrix
* in the form
*
*     | a1,  b1,  c1 |
*     | a2,  b2,  c2 |
*     | a3,  b3,  c3 |
*/
static inline float det3x3(	float a1, float a2, float a3,
							float b1, float b2, float b3,
							float c1, float c2, float c3 )
{
	return	a1 * det2x2( b2, b3, c2, c3 ) -
			b1 * det2x2( a2, a3, c2, c3 ) +
			c1 * det2x2( a2, a3, b2, b3 );
}

//==================================================================
float Matrix44::GetDeterminant() const
{
	float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;

	/* assign to individual variable names to aid selecting */
	/*  correct elements */

	a1 = _m[0][0]; b1 = _m[0][1]; 
	c1 = _m[0][2]; d1 = _m[0][3];

	a2 = _m[1][0]; b2 = _m[1][1]; 
	c2 = _m[1][2]; d2 = _m[1][3];

	a3 = _m[2][0]; b3 = _m[2][1]; 
	c3 = _m[2][2]; d3 = _m[2][3];

	a4 = _m[3][0]; b4 = _m[3][1]; 
	c4 = _m[3][2]; d4 = _m[3][3];

	return	a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4) -
			b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4) +
			c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4) -
			d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
}

//==================================================================
/* 
*   adjoint( original_matrix, inverse_matrix )
* 
*     calculate the adjoint of a 4x4 matrix
*
*      Let  a   denote the minor determinant of matrix A obtained by
*           ij
*
*      deleting the ith row and jth column from A.
*
*                    i+j
*     Let  b   = (-1)    a
*          ij            ji
*
*    The matrix B = (b  ) is the adjoint of A
*                     ij
*/
inline Matrix44 Matrix44::GetAdjoint() const
{
	float a1, a2, a3, a4, b1, b2, b3, b4;
	float c1, c2, c3, c4, d1, d2, d3, d4;

	/* assign to individual variable names to aid  */
	/* selecting correct values  */

	a1 = _m[0][0]; b1 = _m[0][1]; 
	c1 = _m[0][2]; d1 = _m[0][3];

	a2 = _m[1][0]; b2 = _m[1][1]; 
	c2 = _m[1][2]; d2 = _m[1][3];

	a3 = _m[2][0]; b3 = _m[2][1];
	c3 = _m[2][2]; d3 = _m[2][3];

	a4 = _m[3][0]; b4 = _m[3][1]; 
	c4 = _m[3][2]; d4 = _m[3][3];


	Matrix44	out_mat;
	/* row column labeling reversed since we transpose rows & columns */

	out_mat._m[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
	out_mat._m[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
	out_mat._m[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
	out_mat._m[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);

	out_mat._m[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
	out_mat._m[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
	out_mat._m[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
	out_mat._m[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);

	out_mat._m[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
	out_mat._m[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
	out_mat._m[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
	out_mat._m[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);

	out_mat._m[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
	out_mat._m[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
	out_mat._m[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
	out_mat._m[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);

	return out_mat;
}

//==================================================================
/*
Matrix Inversion
by Richard Carling
from "Graphics Gems", Academic Press, 1990
*/

#define SMALL_NUMBER	1.e-8
/* 
*   inverse( original_matrix, inverse_matrix )
* 
*    calculate the inverse of a 4x4 matrix
*
*     -1     
*     A  = ___1__ adjoint A
*         det A
*/

//==================================================================
Matrix44 Matrix44::GetInverse() const
{
	Matrix44	out_inv_mat;

	out_inv_mat = GetAdjoint();

	/*  calculate the 4x4 determinant
	*  if the determinant is zero, 
	*  then the inverse matrix is not unique.
	*/

	float det = GetDeterminant();

	if ( fabs( det ) < SMALL_NUMBER )
	{
		PDEBUG_PRINTF( _T("Non-singular matrix, no inverse !\n") );
		PASSERT( 0 );
	}

	/* scale the adjoint matrix to get the inverse */

	float oo_det = 1.0f / det;

	for (int i=0; i < 4; ++i)
		for(int j=0; j < 4; ++j)
			out_inv_mat._m[i][j] = out_inv_mat._m[i][j] * oo_det;

	return out_inv_mat;
}

//==================================================================
PMath::Quatern PMath::Matrix44::GetQuaternion() const
{
	Quatern	quat;

	float  tr, s, q[4];
	int    i, j, k;

	static const int nxt[3] = {1, 2, 0};


	tr = _m[0][0] + _m[1][1] + _m[2][2];


	// check the diagonal
	if (tr > 0.0f)
	{
		s = sqrtf(tr + 1.0f);
		quat._w = s / 2.0f;
		s = 0.5f / s;
		quat._v3._x = (_m[1][2] - _m[2][1]) * s;
		quat._v3._y = (_m[2][0] - _m[0][2]) * s;
		quat._v3._z = (_m[0][1] - _m[1][0]) * s;
	}
	else
	{		
		// diagonal is negative
		i = 0;
		if (_m[1][1] > _m[0][0]) i = 1;
		if (_m[2][2] > _m[i][i]) i = 2;
		j = nxt[i];
		k = nxt[j];

		s = sqrtf((_m[i][i] - (_m[j][j] + _m[k][k])) + 1.0f);

		q[i] = s * 0.5f;

		if (s != 0.0f) s = 0.5f / s;

		q[3] = (_m[j][k] - _m[k][j]) * s;
		q[j] = (_m[i][j] + _m[j][i]) * s;
		q[k] = (_m[i][k] + _m[k][i]) * s;

		quat._v3._x = q[0];
		quat._v3._y = q[1];
		quat._v3._z = q[2];
		quat._w = q[3];
	}

	return quat;
}